#include <robina_following/robina_following.h>

following::following()
{
    isFollowEnabled = false;
    obstaclExist    = false;

    humanLastPos.x = 1.0;
    humanLastPos.y = 0.0;
}

/******************** follow the operator  **************************/

void following::follow_operator(const robina_human_tracker_lrf::humanTorsoConstPtr &msg)
{
    geometry_msgs::Twist base_cmd = geometry_msgs::Twist();

    double turnRate;
    double backWard_cmd;

    turnRate = ((msg->human_pos_2d_centre.theta) * M_PI)/180;

    if(isFollowEnabled)
    {
        if(msg->human_exist == 0)
        {
            ROS_INFO("NO HUMAN");
            base_cmd.linear.x = 0;
            base_cmd.angular.z = 0;
        }
        else
        {
            /*if(isHumanMoved(msg))
            {
                if(msg->distance < MIN_DISTANCE )
                {
                    ROS_INFO("STOP , i'm close to you ");
                    base_cmd.linear.x = 0.0;
                    base_cmd.angular.z = 0.0;
                }

                if(msg->distance < CRITIICAL_DISTANCE)
                {
                    backWard_cmd = msg->distance * -1;
                    backWard_cmd /= 3;
                    base_cmd.linear.x = backWard_cmd;
                    base_cmd.angular.z = turnRate * turnRate_threshold;

                    humanLastPos.x = msg->human_pos_2d_centre.x;
                    humanLastPos.y = msg->human_pos_2d_centre.y;
                }
                else
                {
                    base_cmd.linear.x = msg->distance;
                    base_cmd.angular.z = turnRate * turnRate_threshold;

                    humanLastPos.x = msg->human_pos_2d_centre.x;
                    humanLastPos.y = msg->human_pos_2d_centre.y;
                }
            }
            else
            {
                ROS_INFO("STOP , human not moving");
                base_cmd.linear.x = 0.0;
                base_cmd.angular.z = 0.0;
            }*/

            if(msg->distance < MIN_DISTANCE)
            {
                if(msg->distance < CRITIICAL_DISTANCE)
                {
                    /*backWard*/
                    backWard_cmd = msg->distance * -1;
                    backWard_cmd /= 3;
                    base_cmd.linear.x = backWard_cmd;
                    base_cmd.angular.z = turnRate * turnRate_threshold;
                }
                else
                {
                    /*stop*/
                    ROS_INFO("STOP , i am in minimum distance of you");
                    base_cmd.linear.x = 0.0;
                    base_cmd.angular.z = turnRate * turnRate_threshold;
                }
            }
            else
            {
                /*following*/
                base_cmd.linear.x = msg->distance;
                base_cmd.angular.z = turnRate * turnRate_threshold;
            }
        }

        if(obstaclExist)
        {
            ROS_INFO("obstacle exist");
            base_cmd.linear.x = 0.0;
            base_cmd.angular.z = 0.0;
        }

        if(backObstacleExist && base_cmd.linear.x < 0)
        {
            ROS_INFO("stop, back obstacle exist");
            base_cmd.linear.x = 0.0;
            base_cmd.angular.z = 0.0;
        }

        cmdPub.publish(base_cmd);
    }

    else
    {
        ROS_INFO("follow is disable");
    }

}

/********************************** is Human moved ********************************************/
/*bool following::isHumanMoved(const robina_humanTracker_LRF::humanTorsoConstPtr &msg)
{
    ROS_INFO("is human moved ?");
    if(     hypot(msg->human_pos_2d_centre.x, msg->human_pos_2d_centre.y) <= ( hypot(humanLastPos.x, humanLastPos.y) + LAST_POSX_THRESHOLD) &&
            hypot(msg->human_pos_2d_centre.x, msg->human_pos_2d_centre.y) >= ( hypot(humanLastPos.x, humanLastPos.y) - LAST_POSX_THRESHOLD ) )
    {
        return true;
    }
    return false;
}
*/
/********************* sonar call back *****************************/

void following::sonarCb(const sensor_msgs::PointCloudConstPtr &msg)
{
    if(     msg->points[3].x <= FRONT_SONAR_MIN_DISTANCE ||
            msg->points[4].x <= FRONT_SONAR_MIN_DISTANCE )
    {
        obstaclExist = true;
    }

    else
    {
        obstaclExist = false;
    }

    if(     msg->points[11].x >= BACK_SONAR_MIN_DISTANCE ||
            msg->points[12].x >= BACK_SONAR_MIN_DISTANCE )
    {
        backObstacleExist = true;
    }
    else
    {
        backObstacleExist = false;
    }
}

/************************* setup ***********************************/

void following::setup()
{    
    obstaclExist = false;

    followMe_srv_server = nh.advertiseService("followMe",
                                              &following::followMe_serviceCB, this);

    turnRobot_srv = nh.advertiseService("turnRobot",
                                        &following::turnRobot_serviceCB, this);

    follow_cmd = nh.subscribe<robina_human_tracker_lrf::humanTorso>("/humanTorso", 1,
                                                                   (boost::function<void(const robina_human_tracker_lrf::humanTorsoConstPtr&)>)
                                                                   boost::bind(&following::follow_operator, this, _1));

    sonar_sub = nh.subscribe <sensor_msgs::PointCloud>("/RosAria/sonar",10,
                                                       (boost::function <void(const sensor_msgs::PointCloudConstPtr&)>)
                                                       boost::bind(&following::sonarCb, this, _1) );

    cmdPub = nh.advertise <geometry_msgs::Twist> ("/cmd_vel", 1000);

    ROS_INFO("initializing speech server completed");
}

/*********************************************************/
bool following::followMe_serviceCB(robina_following::followMe::Request &req,
                                   robina_following::followMe::Response &res)
{
    geometry_msgs::Twist base_cmd = geometry_msgs::Twist();

    ROS_INFO("followMe callback executed");
    
    if(	req.follow == 1)
        isFollowEnabled = true;
    if(req.follow == 0)
    {
        isFollowEnabled = false;
        base_cmd.linear.x = 0.0;
        base_cmd.angular.z = 0.0;
        for(int i=0;i<30;i++)
            cmdPub.publish(base_cmd);
    }
    return true;
}

/**************************************************************/

bool following::turnRobot_serviceCB(robina_following::turnRobot::Request &req,
                                    robina_following::turnRobot::Response &res)
{
    if(req.turnSide.turnRobot == robina_following::turnRobotMsg::turnBack)
    {
        turnRobot(3.14);
    }
    else if(req.turnSide.turnRobot == robina_following::turnRobotMsg::turnRight)
    {
        turnRobot(-1.57);
    }
    else if(req.turnSide.turnRobot == robina_following::turnRobotMsg::turnLeft)
    {
        turnRobot(1.57);
    }
}

/****************************************************************************/

void following::turnRobot(double turnSideAngle)
{
    nav_msgs::OdometryConstPtr current_odometry_msg;
    nav_msgs::OdometryConstPtr intial_odometry_msg;

    double intialYaw;
    double currentYaw;

    tf::Vector3 intial_odometry;
    tfScalar dist;

    intial_odometry_msg = ros::topic::waitForMessage<nav_msgs::Odometry>("/odom");
    intialYaw = tf::getYaw(intial_odometry_msg->pose.pose.orientation);

    do
    {
        current_odometry_msg = ros::topic::waitForMessage<nav_msgs::Odometry>("/odom");
        currentYaw = tf::getYaw(current_odometry_msg->pose.pose.orientation);

        ROS_INFO("initial odometry is %lf", intialYaw);
        ROS_INFO("curret odometry is %lf", currentYaw);

        base_cmd.linear.x = 0.0;
        if(turnSideAngle > 0 )
            base_cmd.angular.z = 0.5;
        else
            base_cmd.angular.z = -0.5;

        cmdPub.publish(base_cmd);

        if(turnSideAngle > 0 )
        {
            if( currentYaw > ( turnSideAngle + intialYaw ) )
                break;
        }
        else if( turnSideAngle < 0 )
        {
            if(currentYaw < ( turnSideAngle + intialYaw ) )
                break;
        }
    } while(1);

    base_cmd.linear.x = 0.0;
    base_cmd.angular.z = 0.0;
    cmdPub.publish(base_cmd);
}

/************************* main *********************************************/

int main (int argc, char** argv)
{
    ros::init(argc, argv,"following");
    ros::NodeHandle nh;
    following f;
    f.setup();
    ros::spin();
    return 0;
}
